#include <vector>
#include <algorithm>
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/CommandTOL.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/SetMavFrame.h>
#include <mavros_msgs/Mavlink.h>

#include <geometry_msgs/TwistStamped.h>

#include <geometry_msgs/QuaternionStamped.h>
#include <geometry_msgs/Vector3Stamped.h>
#include <sensor_msgs/NavSatFix.h>
#include <std_msgs/Float32.h>
#include <std_msgs/UInt8.h>
#include <std_msgs/Bool.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <tf/tf.h>
#include <sensor_msgs/LaserScan.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <sensor_msgs/Joy.h>
#include <sensor_msgs/Imu.h>
#include <std_msgs/Float32MultiArray.h>
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
// #include <sensor_msgs/LaserScan.h>  qaq
#include <mavros_msgs/OverrideRCIn.h>
#include <mavros_msgs/AttitudeTarget.h>
#include <mavros_msgs/Altitude.h>
#include <std_msgs/String.h>
#include <mavros_msgs/RCIn.h>
#include <mavros_msgs/RCOut.h>
#include <mavros_msgs/ParamGet.h>
#include <mavros_msgs/ParamSet.h>

#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Geometry>



//位置控制pid
const float P_pos = 0.5;
const float I_pos = 0.0;
const float D_pos = 0.0;
const float k_pitch = 0.1;
const float k_roll = 0.1;

const float height_set = 17.0;
float height_curr = 0.0;
float current_z_speed = 0.0;




const float deg2rad = 3.1415926535798/180.0;
const float rad2deg = 180.0/3.1415926535798;

float bizhangvx = 0;
float bizhangvy = 0;

bool takeoff_result;
std_msgs::String key_receive;

//navigation define
#define IFXUANTING 1
#define HAVEPOS 1
#define USECIRCLE 0
#define USETAG 1

//if use gps
#define VIO 0
#define GPS 1

//vision odometry to flight controler imu distance
#define DELTA_X -0.1
#define DELTA_Y 0.0
#define DELTA_Z 0.03

#define DEAD_ZONE 0.00 //dead zone, unit: m
#define POS_I_DEAD_ZONE 0.04 //in dead zone, don't intergrate
#define YAW_DEAD_ZONE 0
#define HEIGHT_DEAD_ZONE 0.0001




float yaw_init = 0.0;
//for control
float error_yaw_last = 0.0, error_yaw_integrated = 0.0;
bool saved_update_flag = false, current_update_flag = false, planned_update_flag = false, circle_point_flag = false;
double time_current_update = 0,time_current_update_last = 0,time_current_update_last2 = 0, time_circle_update = 0;


const float I_VEL_LIMIT = 0.025; //the intergrate item limit of position control, limit the expected velocity
const float D_VEL_LIMIT = 0.015;
const float I_ANG_LIMIT = 0.5; //the intergrate item limit of velocity control, limit the expected angle
const float YAW_I_LIMIT = 2.0;
const float YAW_R_LIMIT = 0.015;
const float YAW_RATE_LIMIT = 30.0;
const float BIZHANG_LIMIT = 1.0;

bool dead_zone_flag = false;

//control parameters

const float P_vel = 6.00;
const float I_vel = 1.0;
const float D_vel = 0.00;
const float P_yaw = 0.2;
const float I_yaw = 0.00;
const float D_yaw = 0.00;
const float P_z = 1.00;

 
int channel5_value;
int channel6_value;
int channel7_value;
int channel8_value;


int rc_out_signal_value = 0;

void attitude_pid_control(geometry_msgs::Point current_set_point,geometry_msgs::Point current_local_point,float velocity_limit,float target_yaw, float dead_zone, uint8_t mode);

void position_pid_control_with_tag(geometry_msgs::Point current_set_point,geometry_msgs::Point current_local_point,float velocity_limit,float target_yaw, float dead_zone, uint8_t mode);

void position_pid_control(geometry_msgs::Point current_set_point,geometry_msgs::Point current_local_point,float velocity_limit,float target_yaw, float dead_zone, uint8_t mode);
geometry_msgs::Vector3 toEulerAngle(geometry_msgs::Quaternion quat);
geometry_msgs::Vector3 toEulerAngle_tag(geometry_msgs::Quaternion quat);
geometry_msgs::Point rotation_hehe(geometry_msgs::Vector3 angle);
geometry_msgs::Point limit_velocity(float vx, float vy,float maximum);
geometry_msgs::Quaternion quatMultiply(geometry_msgs::Quaternion q1, geometry_msgs::Quaternion q2);
Eigen::Matrix3d Quaternion2RotationMatrix(const double x,const double y,const double z,const double w);
geometry_msgs::Vector3 R_multi_P(Eigen::Matrix3d R, double x, double y, double z);



geometry_msgs::Vector3 tag_attitude_rpy;
geometry_msgs::Quaternion quaternion_att_control;
sensor_msgs::LaserScan scan_msg;


mavros_msgs::State current_state;
ros::Subscriber circle_sub;
ros::Subscriber tagdect_sub;
ros::Subscriber local_odom_sub;
ros::Subscriber att_target_sub;
ros::Subscriber relheight_sub;
ros::Subscriber keyboard_sub;
ros::Subscriber state_sub;
ros::Subscriber odometrysub;
ros::Subscriber savedmapsub;
ros::Subscriber planWaypointsub;
ros::Subscriber scan_tf_sub;
ros::Subscriber gps_frommaster_sub;
ros::Subscriber rcin_sub;
ros::Subscriber rcout_sub;
ros::Subscriber mavlinkfrom_sub;
ros::Publisher local_pos_pub, local_vel_pub, local_att_pub, rc_ov_pub, ifland_pub;
ros::Publisher plot_msg_pub;

ros::ServiceClient arming_client;
ros::ServiceClient land_client;
ros::ServiceClient set_mode_client;
ros::ServiceClient set_vechile_ned;
ros::ServiceClient get_param_client;
ros::ServiceClient set_param_client;

geometry_msgs::Point current_point;
geometry_msgs::Point current_aid_point;

geometry_msgs::Point search_point;

geometry_msgs::Point saved_point;
geometry_msgs::Point planned_point;
geometry_msgs::Vector3 current_imu_angle;
geometry_msgs::Quaternion current_angle;
geometry_msgs::Quaternion current_aid_angle;


geometry_msgs::Quaternion saved_angle;
geometry_msgs::Quaternion planned_angle;
geometry_msgs::Vector3 curr_angle;
geometry_msgs::Vector3 curr_aid_angle;
geometry_msgs::Vector3 save_angle;
geometry_msgs::Vector3 plan_angle;
float current_yaw_angle;
float current_aid_yaw_angle;
float saved_yaw_angle;
float planned_yaw_angle;

//there will be three level april tag
int tag_size_lv1,tag_size_lv2,tag_size_lv3;
int vehicle_id_;
geometry_msgs::Point tag_point_lv1;
geometry_msgs::Point tag_point_lv2;
geometry_msgs::Point tag_point_lv3;

geometry_msgs::Vector3 tag_angle_lv1;
geometry_msgs::Vector3 tag_angle_lv2;
geometry_msgs::Vector3 tag_angle_lv3;
bool tag_point_any = false;
bool  tag_point_flag_lv1 = false, tag_point_flag_lv2 = false, tag_point_flag_lv3 = false;
double time_tag_update_lv1 = 0, time_tag_update_lv2 = 0, time_tag_update_lv3 = 0;
int circle_t = 0;
float R_circle = 0.5;

geometry_msgs::Point tag_position, convert_tag_position, error_pos_last, error_pos_integrated, attitude_expect, velocity_expected, velocity_ned;
geometry_msgs::PoseStamped pose;
geometry_msgs::TwistStamped msgtwist;
geometry_msgs::Vector3 linear;
geometry_msgs::Vector3 angular;
cv::Point2i center_point(0, 0);
geometry_msgs::Point current_plane_point;
geometry_msgs::Point current_xuanting_point;


template <typename T>
T limit(T a, T max)
{
  if(a > max)
    a = max;
  if(a < -max)
    a = -max;
  return a;
}

typedef struct{
    float latitude;
    float longitude;
    float altitude;
    int vehicleId;
    int command;
} vehicle_pos;


vehicle_pos othervehicle_1;
vehicle_pos othervehicle_2;
vehicle_pos othervehicle_3;
vehicle_pos othervehicle_4;
bool ifland_ = false;
bool self_vehicle_down = false;
bool up_home = false;
bool height_enough = false;
bool  time_vehicle1_pos_flag = false, time_vehicle2_pos_flag = false, time_vehicle3_pos_flag = false, time_vehicle4_pos_flag = false;
double time_vehicle1_pos_update = 0, time_vehicle2_pos_update = 0, time_vehicle3_pos_update = 0 , time_vehicle4_pos_update = 0;

float tag2local_yaw_lv1, tag2local_yaw_lv2, tag2local_yaw_lv3;
float tgt_yaw_lv1, tgt_yaw_lv2, tgt_yaw_lv3;
geometry_msgs::PoseStamped pose_tag_lv1, pose_tag_lv2, pose_tag_lv3;
geometry_msgs::PoseStamped pose_tag_final_f;
geometry_msgs::PoseStamped pose_tag_filter(geometry_msgs::PoseStamped pose, geometry_msgs::PoseStamped pose_f, float alpha_filter);
geometry_msgs::Vector3 threeaxisrot(double r11, double r12, double r21, double r31, double r32);

geometry_msgs::Vector3 quaternion2Euler(geometry_msgs::Quaternion q);
bool first_see_tag = true;
bool tag_pose_inited = false;

geometry_msgs::PoseStamped pose_send;

float height_takeoff;
bool height_takeoff_b = true;

const float camera_offset_x = -0.21;
const float camera_offset_y = 0.0;
const float camera_offset_z = 0.0;


geometry_msgs::Quaternion q_tag_lv1, q_tag_lv2, q_tag_lv3;
geometry_msgs::Quaternion q_cam_to_vehicle;
geometry_msgs::Quaternion q_cam_to_vehicle_multi;
geometry_msgs::Quaternion q_cam_to_vehicle_inverse;
geometry_msgs::Quaternion q_vehicle;
geometry_msgs::Vector3 tag_angle_control;


mavros_msgs::Mavlink cam_trig_mavlinkfrom;
